#ifndef __DALELIB_ROBOT_H__
#define __DALELIB_ROBOT_H__

#include <vector>
#include <map>
#include <dalelib/maths/m4.h>
#include <dalelib/graphics/shader.h>
#include <dalelib/alife/robotpart.h>
#include <dalelib/alife/robotjoint.h>

using namespace std;

namespace dalelib
{

class Robot
{
public:

	typedef vector<RobotPart*> PartList;
	typedef vector<RobotJoint*> JointList;
	typedef map<dGeomID,RobotPart*> GeomPartMap;

protected:

	dWorldID _world;
	dSpaceID _space;
	dJointGroupID _jointgroup;
	PartList _parts;
	JointList _joints;
	GeomPartMap _geompartmap;

public:

	Robot(dWorldID _World, dSpaceID _Space);
	~Robot();
	void clean();
	void reset();
	dWorldID getWorld() { return _world; }
	dSpaceID getSpace() { return _space; }
	int getPartNum() { return _parts.size(); }
	RobotPart* getPart(const int _Index) { return _parts[_Index]; }
	RobotPart* getPart(dGeomID _geom) { return _geompartmap[_geom]; }
	PartList& getPartList() { return _parts; }
	int getJointNum() { return _joints.size(); }
	RobotJoint* getJoint(const int _Index) { return _joints[_Index]; }
	JointList& getJointList() { return _joints; }
	dJointGroupID getJointGroup() { return _jointgroup; }
	void translate(const v3 _Pos);
	void rotate(m3 _Rot);
	v3 getAveragePosition();
//	void setTouch(dGeomID _geom,const float _Value) { _geompartmap[_geom]->setTouch(_Value); }

	RobotJoint* addHingeJoint(const int _B0, const int _B1, const float _Strength, const v3 _Pos, const v3 _Dir);

	RobotPart* addCapsule(const float _Radius, const v3 _P0, const v3 _P1, const float _Density);
	RobotPart* addBox(const float _Radius, const v3 _P0, const v3 _P1, const float _Density);

	void update();
	void draw();
	void drawJoints();
	void load(const string _FileName);

};

} // namespace dalelib

#endif
